/*
 * FunnelDeque.cpp
 *
 *  Created on: 16/08/2012
 *      Author: Aeradon
 */

#include "../include/FunnelDeque.h"
#include "../include/navinode.hpp"

//constructor - initializes the apex of the funnel
//to the point passed, and the unit radius
FunnelDeque::FunnelDeque(float x, float y, float r)
{
	nApex = new FunnelNode(x, y);
	nLeft = nApex;
	nRight = nApex;
	m_tApexType = POINT;
	m_dRadius = r;
}

//constructor - initializes the apex of the funnel
//to the coordinates passed, and the unit radius
FunnelDeque::FunnelDeque(const Point<T>& point, float r)
{
	nApex = new FunnelNode(point);
	nLeft = nApex;
	nRight = nApex;
	m_tApexType = POINT;
	m_dRadius = r;
}

//destructor - deletes any funnel nodes in the deque
FunnelDeque::~FunnelDeque()
{
	FunnelNode *current = nLeft;
	while (current != NULL)
	{
		FunnelNode *temp = current;
		current = current->Right();
		delete temp;
	}
}

//Adds a new point to the funnel, adds to the existing path if the apex moves
//type = RightTangent => point is being added on left side
//type = LeftTangent => point is being added on right side
//type = POINT => point is the end of the channel
void FunnelDeque::Add(const Point<T>& p, CORNER_TYPE type, Polygon2D<T>& path)
{
	//the new funnel node containing the new point
	FunnelNode *next = new FunnelNode(p);
	//if it's being added on the left side
	//(operations are just reversed for the other side)
	if (type == RIGHT_TANGENT)
	{
		//loop until the point is added to the funnel
		while (true)
		{
			//if the apex is the only point in the funnel,
			//simply add the point to the appropriate side
			if (nLeft == nRight)
			{
				next->Right(nApex);
				nApex->Left(next);
				nLeft = next;
				break;
			}
			float x_1, y_1, x_2, y_2;
			CORNER_TYPE type_1, type_2;
			//if there are no points on the left side of the funnel,
			//get the wedge going to the opposite side of the apex
			if (nLeft == nApex)
			{
				x_1 = nLeft->X();
				y_1 = nLeft->Y();
				x_2 = nLeft->Right()->X();
				y_2 = nLeft->Right()->Y();
				type_1 = m_tApexType;
				type_2 = LEFT_TANGENT;
			}
			//otherwise get the wedge on this side of the apex
			else
			{
				x_2 = nLeft->X();
				y_2 = nLeft->Y();
				x_1 = nLeft->Right()->X();
				y_1 = nLeft->Right()->Y();
				type_2 = RIGHT_TANGENT;
				if (nLeft->Right() == nApex)
				{
					type_1 = m_tApexType;
				}
				else
				{
					type_1 = RIGHT_TANGENT;
				}
			}
			//calculate the angle associated with this wedge
			float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			//also calculate the distance between these two points
			float length1 = Distance(x_1, y_1, x_2, y_2);
			//and that to the new point (special case for the modofied funnel algorithm)
			float length2 = Distance(x_1, y_1, p.x, p.y);
			//now get the wedge leading to the new point
			x_1 = nLeft->X();
			y_1 = nLeft->Y();
			if (nApex == nLeft)
			{
				type_1 = m_tApexType;
			}
			else
			{
				type_1 = RIGHT_TANGENT;
			}
			x_2 = p.x;
			y_2 = p.y;
			type_2 = type;
			//and the angle associated with it
			float toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			//compare the two angles
			float diff = wedge - toPOINT;
			diff += (diff <= 0.0f - PFRUtils::PI) ? 2.0f * PFRUtils::PI : (diff > PFRUtils::PI) ? -2.0f * PFRUtils::PI : 0.0f;
			//if the existing wedge is counterclockwise of that to the point
			if (diff < 0.0f)
			{
				//add the new point on this end of the funnel
				nLeft->Left(next);
				next->Right(nLeft);
				nLeft = next;
				break;
			}
			//if it is clockwise,
			else
			{
				//check if we are popping the apex
				if (nLeft == nApex)
				{
					//if the point being added is closer than the one being popped,
					if (length2 < length1)
					{
						//make the new point the apex instead of the one on the opposite side
						//(special case for the modified funnel algorithm)
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType, p.x, p.y, type);
						AddPoint(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddPoint(p.x, p.y, type, angle, path);
						next->Right(nApex->Right());
						nApex->Right()->Left(next);
						delete nApex;
						nApex = next;
						nLeft = next;
						m_tApexType = type;
						break;
					}
					//otherwise,
					else
					{
						//move the apex to the right and add the segment between
						//the old and new apexes to the path so far
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType,
							nApex->Right()->X(), nApex->Right()->Y(), LEFT_TANGENT);
						AddPoint(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddPoint(nApex->Right()->X(), nApex->Right()->Y(), LEFT_TANGENT, angle, path);
						nApex = nApex->Right();
						m_tApexType = LEFT_TANGENT;
					}
				}
				//pop the leftmost point off of the funnel
				FunnelNode *temp = nLeft;
				nLeft = nLeft->Right();
				nLeft->Left(NULL);
				delete temp;
			}
		}
	}
	//otherwise, if adding a point on the right side,
	//continue as before, with sides reversed
	//adding a point also ends up here - 
	//we do this to pop off all the necessary points off of
	//the right side of the funnel, so at the end we can just
	//add to the path the points between the apex and the
	//right side of the funnel (done at the end)
	else
	{
		while (true)
		{
			if (nLeft == nRight)
			{
				next->Left(nApex);
				nApex->Right(next);
				nRight = next;
				break;
			}
			float x_1, y_1, x_2, y_2;
			CORNER_TYPE type_1, type_2;
			if (nRight == nApex)
			{
				x_1 = nRight->X();
				y_1 = nRight->Y();
				x_2 = nRight->Left()->X();
				y_2 = nRight->Left()->Y();
				type_1 = m_tApexType;
				type_2 = RIGHT_TANGENT;
			}
			else
			{
				x_2 = nRight->X();
				y_2 = nRight->Y();
				x_1 = nRight->Left()->X();
				y_1 = nRight->Left()->Y();
				type_2 = LEFT_TANGENT;
				if (nRight->Left() == nApex)
				{
					type_1 = m_tApexType;
				}
				else
				{
					type_1 = LEFT_TANGENT;
				}
			}
			float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			float length1 = Distance(x_1, y_1, x_2, y_2);
			float length2 = Distance(x_1, y_1, p.x, p.y);
			x_1 = nRight->X();
			y_1 = nRight->Y();
			if (nApex == nRight)
			{
				type_1 = m_tApexType;
			}
			else
			{
				type_1 = LEFT_TANGENT;
			}
			x_2 = p.x;
			y_2 = p.y;
			type_2 = type;
			float toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			float diff = wedge - toPOINT;
			diff += (diff <= 0.0f - PFRUtils::PI) ? 2.0f * PFRUtils::PI : (diff > PFRUtils::PI) ? -2.0f * PFRUtils::PI : 0.0f;
			if (diff < 0.0f)
			{
				if (nRight == nApex)
				{
					if (length2 < length1)
					{
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType, p.x, p.y, type);
						AddPoint(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddPoint(p.x, p.y, type, angle, path);
						next->Left(nApex->Left());
						nApex->Left()->Right(next);
						delete nApex;
						nApex = next;
						nRight = next;
						m_tApexType = type;
						break;
					}
					else
					{
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType,
							nApex->Left()->X(), nApex->Left()->Y(), RIGHT_TANGENT);
						AddPoint(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddPoint(nApex->Left()->X(), nApex->Left()->Y(), RIGHT_TANGENT, angle, path);
						nApex = nApex->Left();
						m_tApexType = RIGHT_TANGENT;
					}
				}
				FunnelNode *temp = nRight;
				nRight = nRight->Left();
				nRight->Right(NULL);
				delete temp;
			}
			else
			{
				nRight->Right(next);
				next->Left(nRight);
				nRight = next;
				break;
			}
		}
	}
	//this is where the points between the apex
	//and the right side o fthe funnel are added to the path
	//when adding the final point to the channel
	if (type == POINT)
	{
		FunnelNode *Current = nApex;
		while (Current != nRight)
		{
			float x_1, y_1, x_2, y_2, toPOINT;
			CORNER_TYPE type_1, type_2;
			type_1 = (Current == nApex) ? m_tApexType : LEFT_TANGENT;
			type_2 = (Current->Right() == nRight) ? POINT : LEFT_TANGENT;
			x_1 = Current->X();
			y_1 = Current->Y();
			Current = Current->Right();
			x_2 = Current->X();
			y_2 = Current->Y();
			toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			AddPoint(x_1, y_1, type_1, toPOINT, path);
			AddPoint(x_2, y_2, type_2, toPOINT, path);
		}
	}
}

void FunnelDeque::Add(const Vector2D& v, CORNER_TYPE type, std::list<Vector2D>* path)
{
	//the new funnel node containing the new point
	FunnelNode *next = new FunnelNode(v.x, v.y);
	//if it's being added on the left side
	//(operations are just reversed for the other side)
	if (type == RIGHT_TANGENT)
	{
		//loop until the point is added to the funnel
		while (true)
		{
			//if the apex is the only point in the funnel,
			//simply add the point to the appropriate side
			if (nLeft == nRight)
			{
				next->Right(nApex);
				nApex->Left(next);
				nLeft = next;
				break;
			}
			float x_1, y_1, x_2, y_2;
			CORNER_TYPE type_1, type_2;
			//if there are no points on the left side of the funnel,
			//get the wedge going to the opposite side of the apex
			if (nLeft == nApex)
			{
				x_1 = nLeft->X();
				y_1 = nLeft->Y();
				x_2 = nLeft->Right()->X();
				y_2 = nLeft->Right()->Y();
				type_1 = m_tApexType;
				type_2 = LEFT_TANGENT;
			}
			//otherwise get the wedge on this side of the apex
			else
			{
				x_2 = nLeft->X();
				y_2 = nLeft->Y();
				x_1 = nLeft->Right()->X();
				y_1 = nLeft->Right()->Y();
				type_2 = RIGHT_TANGENT;
				if (nLeft->Right() == nApex)
				{
					type_1 = m_tApexType;
				}
				else
				{
					type_1 = RIGHT_TANGENT;
				}
			}
			//calculate the angle associated with this wedge
			float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			//also calculate the distance between these two points
			float length1 = Distance(x_1, y_1, x_2, y_2);
			//and that to the new point (special case for the modofied funnel algorithm)
			float length2 = Distance(x_1, y_1, v.x, v.y);
			//now get the wedge leading to the new point
			x_1 = nLeft->X();
			y_1 = nLeft->Y();
			if (nApex == nLeft)
			{
				type_1 = m_tApexType;
			}
			else
			{
				type_1 = RIGHT_TANGENT;
			}
			x_2 = v.x;
			y_2 = v.y;
			type_2 = type;
			//and the angle associated with it
			float toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			//compare the two angles
			float diff = wedge - toPOINT;
			diff += (diff <= 0.0f - PFRUtils::PI) ? 2.0f * PFRUtils::PI : (diff > PFRUtils::PI) ? -2.0f * PFRUtils::PI : 0.0f;
			//if the existing wedge is counterclockwise of that to the point
			if (diff < 0.0f)
			{
				//add the new point on this end of the funnel
				nLeft->Left(next);
				next->Right(nLeft);
				nLeft = next;
				break;
			}
			//if it is clockwise,
			else
			{
				//check if we are popping the apex
				if (nLeft == nApex)
				{
					//if the point being added is closer than the one being popped,
					if (length2 < length1)
					{
						//make the new point the apex instead of the one on the opposite side
						//(special case for the modified funnel algorithm)
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType, v.x, v.y, type);
						AddVec2D(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddVec2D(v.x, v.y, type, angle, path);
						next->Right(nApex->Right());
						nApex->Right()->Left(next);
						delete nApex;
						nApex = next;
						nLeft = next;
						m_tApexType = type;
						break;
					}
					//otherwise,
					else
					{
						//move the apex to the right and add the segment between
						//the old and new apexes to the path so far
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType,
							nApex->Right()->X(), nApex->Right()->Y(), LEFT_TANGENT);
						AddVec2D(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddVec2D(nApex->Right()->X(), nApex->Right()->Y(), LEFT_TANGENT, angle, path);
						nApex = nApex->Right();
						m_tApexType = LEFT_TANGENT;
					}
				}
				//pop the leftmost point off of the funnel
				FunnelNode *temp = nLeft;
				nLeft = nLeft->Right();
				nLeft->Left(NULL);
				delete temp;
			}
		}
	}
	//otherwise, if adding a point on the right side,
	//continue as before, with sides reversed
	//adding a point also ends up here -
	//we do this to pop off all the necessary points off of
	//the right side of the funnel, so at the end we can just
	//add to the path the points between the apex and the
	//right side of the funnel (done at the end)
	else if (type == LEFT_TANGENT)
	{
		while (true)
		{
			if (nLeft == nRight)
			{
				next->Left(nApex);
				nApex->Right(next);
				nRight = next;
				break;
			}
			float x_1, y_1, x_2, y_2;
			CORNER_TYPE type_1, type_2;
			if (nRight == nApex)
			{
				x_1 = nRight->X();
				y_1 = nRight->Y();
				x_2 = nRight->Left()->X();
				y_2 = nRight->Left()->Y();
				type_1 = m_tApexType;
				type_2 = RIGHT_TANGENT;
			}
			else
			{
				x_2 = nRight->X();
				y_2 = nRight->Y();
				x_1 = nRight->Left()->X();
				y_1 = nRight->Left()->Y();
				type_2 = LEFT_TANGENT;
				if (nRight->Left() == nApex)
				{
					type_1 = m_tApexType;
				}
				else
				{
					type_1 = LEFT_TANGENT;
				}
			}
			float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			float length1 = Distance(x_1, y_1, x_2, y_2);
			float length2 = Distance(x_1, y_1, v.x, v.y);
			x_1 = nRight->X();
			y_1 = nRight->Y();
			if (nApex == nRight)
			{
				type_1 = m_tApexType;
			}
			else
			{
				type_1 = LEFT_TANGENT;
			}
			x_2 = v.x;
			y_2 = v.y;
			type_2 = type;
			float toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
			float diff = wedge - toPOINT;
			diff += (diff <= 0.0f - PFRUtils::PI) ? 2.0f * PFRUtils::PI : (diff > PFRUtils::PI) ? -2.0f * PFRUtils::PI : 0.0f;
			if (diff < 0.0f)
			{
				if (nRight == nApex)
				{
					if (length2 < length1)
					{
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType, v.x, v.y, type);
						AddVec2D(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddVec2D(v.x, v.y, type, angle, path);
						next->Left(nApex->Left());
						nApex->Left()->Right(next);
						delete nApex;
						nApex = next;
						nRight = next;
						m_tApexType = type;
						break;
					}
					else
					{
						float angle = Angle(nApex->X(), nApex->Y(), m_tApexType,
							nApex->Left()->X(), nApex->Left()->Y(), RIGHT_TANGENT);
						AddVec2D(nApex->X(), nApex->Y(), m_tApexType, angle, path);
						AddVec2D(nApex->Left()->X(), nApex->Left()->Y(), RIGHT_TANGENT, angle, path);
						nApex = nApex->Left();
						m_tApexType = RIGHT_TANGENT;
					}
				}
				FunnelNode *temp = nRight;
				nRight = nRight->Left();
				nRight->Right(NULL);
				delete temp;
			}
			else
			{
				nRight->Right(next);
				next->Left(nRight);
				nRight = next;
				break;
			}
		}
	}
	//this is where the points between the apex
	//and the right side o fthe funnel are added to the path
	//when adding the final point to the channel
//	else if (type == POINT)
//	{
//		FunnelNode *Current = nApex;
//		while (Current != nRight)
//		{
//			float x_1, y_1, x_2, y_2, toPOINT;
//			CORNER_TYPE type_1, type_2;
//			type_1 = (Current == nApex) ? m_tApexType : LEFT_TANGENT;
//			type_2 = (Current->Right() == nRight) ? POINT : LEFT_TANGENT;
//			x_1 = Current->X();
//			y_1 = Current->Y();
//			Current = Current->Right();
//			x_2 = Current->X();
//			y_2 = Current->Y();
//			toPOINT = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
//			AddVec2D(x_1, y_1, type_1, toPOINT, path);
//			AddVec2D(x_2, y_2, type_2, toPOINT, path);
//		}
//	}
}


//returns the length of a path found by this funnel algorithm with the current radius
float FunnelDeque::Length(Polygon2D<T> path)
{
	//only paths with an even number of points are valid
	if ((path.size() % 2) != 0)
	{
		return 0.0f;
	}
	float length = 0.0f;
	//go through the straight segments and total their lengths
	for (unsigned int i = 0; i < path.size(); i += 2)
	{
		float xDiff = path[i + 1].x - path[i].x;
		float yDiff = path[i + 1].y - path[i].y;
		length += sqrt(xDiff * xDiff + yDiff * yDiff);
	}
	//go through the curved segments and add their length
	//(the difference of the angles of the
	// surrounding segments, times the unit radius)
	for (unsigned int i = 1; i < path.size() - 3; i += 2)
	{
		float angle1 = atan2(path[i + 1].y - path[i].y, path[i + 1].x - path[i].x);
		float angle2 = atan2(path[i + 3].y - path[i + 2].y, path[i + 3].x - path[i + 2].x);
		float diff = angle1 - angle2;
		diff += (diff <= 0.0f - PFRUtils::PI) ? 2.0f * PFRUtils::PI : (diff > PFRUtils::PI) ? -2.0f * PFRUtils::PI : 0.0f;
		length += m_dRadius * fabs(diff);
	}
	return length;
}

//returns the angle from x_1, y_1 to x_2, y_2, given the types
float FunnelDeque::Angle(float x_1, float y_1, CORNER_TYPE type_1, float x_2, float y_2, CORNER_TYPE type_2)
{
	//simply calls the appropriate function based
	//on the types of the two points
	if (type_1 == POINT)
	{
		return PointToTangent(x_1, y_1, x_2, y_2, (type_2 == RIGHT_TANGENT));
	}
	else if (type_2 == POINT)
	{
		float angle = PointToTangent(x_2, y_2, x_1, y_1, (type_1 == LEFT_TANGENT));
		return AngleRange(angle + PFRUtils::PI);
	}
	else if (type_1 != type_2)
	{
		return ToTangentAlt(x_1, y_1, x_2, y_2, (type_1 == LEFT_TANGENT));
	}
	else
	{
		return ToTangent(x_1, y_1, x_2, y_2);
	}
}

//adds a point to the path given a coordinate, its type,
//and the angle of the segment attached
void FunnelDeque::AddPoint(float x, float y, CORNER_TYPE type, float angle, Polygon2D<T> &path)
{
	//if it's a point type, just add it unchanged
	if (type == POINT)
	{
		path.push_back(Point<T>(x,y));
	}
	//otherwise,
	else
	{
		//move the unit radius from the point to one side or the other
		float theta = AngleRange(angle + PFRUtils::PI / ((type == LEFT_TANGENT) ? 2.0f : -2.0f));
		path.push_back(Point<T>(x + cos(theta) * m_dRadius, y + sin(theta) * m_dRadius));
	}
}

//adds a point to the path given a coordinate, its type,
//and the angle of the segment attached
void FunnelDeque::AddVec2D(float x, float y, CORNER_TYPE type, float angle, std::list<Vector2D>* path)
{
	//if it's a point type, just add it unchanged
	Vector2D vecToAdd;
	if (type == POINT)
	{
		vecToAdd.Set(x,y);
		if(path->empty() || vecToAdd != path->back())
			path->push_back(Vector2D(x,y));
	}
	else //move the unit radius from the point to one side or the other
	{
		float theta = AngleRange(angle + PFRUtils::PI / ((type == LEFT_TANGENT) ? 2.0f : -2.0f));
		vecToAdd.Set(x + cos(theta) * m_dRadius, y + sin(theta) * m_dRadius);
		// XXX WORKARROUND to angle NAN || INF
		if (isnan(vecToAdd.x) || isinf(vecToAdd.x))
			return;
		path->push_back(vecToAdd);
	}
}

//takes an angle value and restricts it to the range (-PI, PI)
float FunnelDeque::AngleRange(float theta)
{
	return ((theta > PFRUtils::PI) ? (theta - 2.0f * PFRUtils::PI) :
		(theta <= 0.0f - PFRUtils::PI) ? (theta + 2.0f * PFRUtils::PI) : theta);
}

//gets the distance between the points (x_1, y_1) and (x_2, y_2)
float FunnelDeque::Distance(float x_1, float y_1, float x_2, float y_2)
{
	float x = x_2 - x_1;
	float y = y_2 - y_1;
	return sqrt(x * x + y * y);
}

//gets the angle of the line tangent to two points of the same type
float FunnelDeque::ToTangent(float x_1, float y_1, float x_2, float y_2)
{
	return atan2(y_2 - y_1, x_2 - x_1);
}

//gets the angle of the line tangent to a point
//of type LeftTangent and one of type RightTangent
float FunnelDeque::ToTangentAlt(float x_1, float y_1, float x_2, float y_2, bool LeftRight)
{
	float h = Distance(x_1, y_1, x_2, y_2) / 2.0f;
	float l = sqrt(h * h - m_dRadius * m_dRadius);
	float interior = atan(m_dRadius / l);
	float absolute = atan2(y_2 - y_1, x_2 - x_1);
	if (LeftRight)
	{
		interior = -interior;
	}
	return AngleRange(absolute + interior);
}

//gets the angle of the line going through one point
//and running tangent to another
float FunnelDeque::PointToTangent(float x_1, float y_1, float x_2, float y_2, bool Right)
{
	float h = Distance(x_1, y_1, x_2, y_2);
	float l = sqrt(h * h - m_dRadius * m_dRadius);
	float interior = atan(m_dRadius / l);
	float absolute = atan2(y_2 - y_1, x_2 - x_1);
	if (Right)
	{
		interior = -interior;
	}
	return AngleRange(absolute + interior);
}

#ifdef _DEBUG
#include <cstdio>
// prints the funnel (used for debugging)
void FunnelDeque::PrintFunnel()
{
	FunnelNode *Current = nLeft;
	while (Current != NULL)
	{
		printf("\t(%2.2f, %2.2f)", Current->X(), Current->Y());
		if (Current == nLeft)
		{
			printf(" <-- Left");
		}
		if (Current == nApex)
		{
			printf(" <-- Apex");
		}
		if (Current == nRight)
		{
			printf(" <-- Right");
		}
		printf("\n"); fflush(stdout);
		Current = Current->Right();
	}
}

// prints the path and then the funnel (used for debugging)
void FunnelDeque::PrintPathAndFunnel(std::list<Vector2D>* path)
{
	printf("Path = {");
	for(std::list<Vector2D>::iterator itr = path->begin();
			itr != path->end();
			++itr)
	{
		printf("\t(%2.2f, %2.2f), ", (*itr).x, (*itr).y);
	}
	printf("}\n"); fflush(stdout);
	PrintFunnel();
}
#endif
